#include "JPhysicsMaths.h"

/*
float JPMaths::dotProduct(const JPVector3 &vector1, const JPVector3 &vector2)
{
	return (vector1.x*vector2.x)+(vector1.y*vector2.y)+(vector1.z*vector2.z);
}

JPVector3 JPMaths::crossProduct(const JPVector3 &v1, const JPVector3 &v2)
{
	return JPVector3( (v1.y * v2.z)-(v1.z * v2.y),  // |   i   j    k   |
		(v1.z * v2.x)-(v1.x * v2.z),				// | v1.x v1.y v1.z |
		(v1.x * v2.y)-(v1.y * v2.x));				// | v2.x v2.y v2.z |
}

float JPMaths::calcMagnitude(const JPVector3 &vector)
{
	return sqrtf(dotProduct(vector, vector));
}

JPVector3 JPMaths::unitVector(const JPVector3 &vector)
{
	float magnitude = calcMagnitude(vector);

	return JPVector3(vector.x/magnitude, vector.y/magnitude, vector.z/magnitude);
}

float JPMaths::length(const JPQuaternion &quat)
{
	return sqrt(quat.x * quat.x + quat.y * quat.y +
		quat.z * quat.z + quat.w * quat.w);
}

JPQuaternion JPMaths::normalise(const JPQuaternion &quat)
{
	float L = length(quat);

	JPQuaternion unit;

	unit.x = quat.x / L;
	unit.y = quat.y / L;
	unit.z = quat.z / L;
	unit.w = quat.w / L;

	return unit;
}

JPQuaternion JPMaths::conjugate(const JPQuaternion &quat)
{
	JPQuaternion conj;

	conj.x = -quat.x;
	conj.y = -quat.y;
	conj.z = -quat.z;
	conj.w = quat.w;

	return conj;
}

JPQuaternion JPMaths::multiply(const JPQuaternion &A, const JPQuaternion &B)
{
	JPQuaternion C;

	C.x = A.w*B.x + A.x*B.w + A.y*B.z - A.z*B.y;
	C.y = A.w*B.y - A.x*B.z + A.y*B.w + A.z*B.x;
	C.z = A.w*B.z + A.x*B.y - A.y*B.x + A.z*B.w;
	C.w = A.w*B.w - A.x*B.x - A.y*B.y - A.z*B.z;
	return C;

}

JPQuaternion JPMaths::vecToQuat(const JPVector3 &vec)
{
	JPQuaternion q;
	q.x = vec.x;
	q.y = vec.y;
	q.z = vec.z;
	q.w = 0.0f;

	return q;
}

JPVector3 JPMaths::quatRotation(const JPVector3 &vector, const JPVector3 &axis, float angle)
{
	JPQuaternion R, result;

	R.x = axis.x * sin(angle/2.0f);
	R.y = axis.y * sin(angle/2.0f);
	R.z = axis.z * sin(angle/2.0f);
	R.w = cos(angle/2.0f);

	JPQuaternion conjR = conjugate(R);

	JPQuaternion v = vecToQuat(vector);                      // v is the point we wish to rotate

	//Result = R * V * R'
	JPQuaternion VmultConjR = multiply(v, conjR);          // V * R'
	result = multiply(R, VmultConjR);                    // R multipled by the above
	// Quats are 'associative'
	return JPVector3(result.x, result.y, result.z);        // return the rotated vector
}

float JPMaths::determinant(const JPMatrix &m)
{
	float det;

	det = m.address[0][0]*(m.address[1][1]*m.address[2][2] - m.address[1][2]*m.address[2][1]) -
		m.address[0][1]*(m.address[1][0]*m.address[2][2] - m.address[1][2]*m.address[2][0]) +
		m.address[0][2]*(m.address[1][0]*m.address[2][1] - m.address[1][1]*m.address[2][0]);

	return det;
}

JPMatrix JPMaths::invertMatrix(const JPMatrix &m)
{
	JPMatrix N, inverse;
	float det = determinant(m);

	N.address[0][0] = m.address[1][1]*m.address[2][2] - m.address[1][2]*m.address[2][1];
	N.address[0][1] = m.address[1][0]*m.address[2][2] - m.address[1][2]*m.address[2][0];
	N.address[0][2] = m.address[1][0]*m.address[2][1] - m.address[1][1]*m.address[2][0];

	N.address[1][0] = m.address[0][1]*m.address[2][2] - m.address[0][2]*m.address[2][1];
	N.address[1][1] = m.address[0][0]*m.address[2][2] - m.address[0][2]*m.address[2][0];
	N.address[1][2] = m.address[0][0]*m.address[2][1] - m.address[0][1]*m.address[2][0];

	N.address[2][0] = m.address[0][1]*m.address[1][2] - m.address[0][2]*m.address[1][1];
	N.address[2][1] = m.address[0][0]*m.address[1][2] - m.address[0][2]*m.address[1][0];
	N.address[2][2] = m.address[0][0]*m.address[1][1] - m.address[0][1]*m.address[1][0];

	inverse = N/det;

	return inverse;
}
*/